#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#include "typedefs.h"
#include "hal.h"
#include "phy.h"
#include "phy_tx.h"
#include "phy_rx.h"

/* dB to linear conversion */
#define MATH_MAX_Q4dB_INPUT           915  // keep linear output in <9.12>
#define MATH_LOG2_10_DIV10_Q16      21771  // log2(10)/10 in Q16
#define MATH_LN_2_Q14               11357  // ln(2) in Q14
#define MATH_1_3_Q15                10923  // 1/3 in Q15
#define MATH_1_Q30                0x40000000  // 1 in Q30

/* cos/sin */
#define	MATH_PI  		    12868  // 4096*PI (in Q12)

/* Constants in Q14 */
/* Note: sin(3pi/8)=cos(pi/8), cos(3pi/8)=sin(pi/8),
         sin(pi/8)-sin(3pi/8)=-(cos(pi/8)-cos(3pi/8),
                 sin(pi/8)+sin(3pi/8)=  cos(pi/8)+cos(3pi/8)
 */
#define SIN_PI_8       6270
#define COS_PI_8      15137
#define COS_PIP3PI_8  21407
#define COS_PIM3PI_8   8867

extern PHY_rxDbg_t     PHY_rxDbg_s;
extern PHY_rxData_t    PHY_rxData_s;
extern PHY_tx_data_t   PHY_tx_data_s;
extern ComplexShort PHY_rxDnCvtLut[128];

extern FSM_stateMachine_t PHY_txSm;
extern FSM_actionCell_t PHY_rxSmTop;
extern FSM_actionCell_t PHY_rxSsmIacq;
extern FSM_actionCell_t PHY_rxSsmLstn;
extern FSM_actionCell_t PHY_rxSsmSchd;

SINT16 tran_hist[128] = {0};
SINT16 old_metric[16] = {0};
SINT16 new_metric[16] = {0};
SINT16 trn_tmp[128] = {0};

SINT16 *trn_s1_p;
SINT16 *trn_s2_p;
SINT16 *trn_w1_p;
SINT16 *trn_w2_p;

PHY_eqz_data_t   PHY_eqz_data_s;

PHY_afeBuf_t PHY_afeBuf_s;

SINT16 PHY_aagcAlgAGainIdx[3] = 
{
	4,  // need -4 dB later
	4,  // binary gain 1  = 0 dB
	4,  // binary gain 16 = 12 dB
};


SINT16 PHY_aagcTgtPower[5] = 
{
	39,
	36,
	39,
	54
};

const int16 PHY_dbpsk[2] = {1, -1};

PHY_scm_handle_t PHY_scm_handle_s;
PHY_mod_handle_t PHY_mod_handle_s;
PHY_descm_handle_t PHY_descm_handle_s;
PHY_bandCfg_t PHY_bandCfg_s;

const Uint16 PHY_pref[127] =
{
	0,   0,   0,   0,   1,   1,   1,   0,   1,   1,   1,   1,   0,   0,   1,   0,   1,   1,   0,   0,
	1,   0,   0,   1,   0,   0,   0,   0,   0,   0,   1,   0,   0,   0,   1,   0,   0,   1,   1,   0,
	0,   0,   1,   0,   1,   1,   1,   0,   1,   0,   1,   1,   0,   1,   1,   0,   0,   0,   0,   0,
	1,   1,   0,   0,   1,   1,   0,   1,   0,   1,   0,   0,   1,   1,   1,   0,   0,   1,   1,   1,
	1,   0,   1,   1,   0,   1,   0,   0,   0,   0,   1,   0,   1,   0,   1,   0,   1,   1,   1,   1,
	1,   0,   1,   0,   0,   1,   0,   1,   0,   0,   0,   1,   1,   0,   1,   1,   1,   0,   0,   0,
	1,   1,   1,   1,   1,   1,   1
};


/* Interleaver matrix */
PHY_intlv_t PHY_intlvCfg_s = 
{
	{
		6, 
		6, 
		6, 
		9 
	},
	{
		7, 
		8, 
		16, 
		16 
	}
};


/* Interleaver matrix */
PHY_intlv_t PHY_intlvBlkCfg_s =
{
	{
		24,
		24,
		24,
		36
	},
	{
		7,
		8,
		16,
		16
	}
};

//sin(i*pi/512)
SINT16 cfft16_table[] = 
{
	0,      201,    402,    603,    804,    1005,   1206,   1407,
	1608,   1809,   2009,   2210,   2410,   2611,   2811,   3012,
	3212,   3412,   3612,   3811,   4011,   4210,   4410,   4609,
	4808,   5007,   5205,   5404,   5602,   5800,   5998,   6195,
	6393,   6590,   6786,   6983,   7179,   7375,   7571,   7767,
	7962,   8157,   8351,   8545,   8739,   8933,   9126,   9319,
	9512,   9704,   9896,  10087,  10278,  10469,  10659,  10849,
	11039,  11228,  11417,  11605,  11793,  11980,  12167,  12353,
	12539,  12725,  12910,  13094,  13279,  13462,  13645,  13828,
	14010,  14191,  14372,  14553,  14732,  14912,  15090,  15269,
	15446,  15623,  15800,  15976,  16151,  16325,  16499,  16673,
	16846,  17018,  17189,  17360,  17530,  17700,  17869,  18037,
	18204,  18371,  18537,  18703,  18868,  19032,  19195,  19357,
	19519,  19680,  19841,  20000,  20159,  20317,  20475,  20631,
	20787,  20942,  21096,  21250,  21403,  21554,  21705,  21856,
	22005,  22154,  22301,  22448,  22594,  22739,  22884,  23027,
	23170,  23311,  23452,  23592,  23731,  23870,  24007,  24143,
	24279,  24413,  24547,  24680,  24811,  24942,  25072,  25201,
	25329,  25456,  25582,  25708,  25832,  25955,  26077,  26198,
	26319,  26438,  26556,  26674,  26790,  26905,  27019,  27133,
	27245,  27356,  27466,  27575,  27683,  27790,  27896,  28001,
	28105,  28208,  28310,  28411,  28510,  28609,  28706,  28803,
	28898,  28992,  29085,  29177,  29268,  29358,  29447,  29534,
	29621,  29706,  29791,  29874,  29956,  30037,  30117,  30195,
	30273,  30349,  30424,  30498,  30571,  30643,  30714,  30783,
	30852,  30919,  30985,  31050,  31113,  31176,  31237,  31297,
	31356,  31414,  31470,  31526,  31580,  31633,  31685,  31736,
	31785,  31833,  31880,  31926,  31971,  32014,  32057,  32098,
	32137,  32176,  32213,  32250,  32285,  32318,  32351,  32382,
	32412,  32441,  32469,  32495,  32521,  32545,  32567,  32589,
	32609,  32628,  32646,  32663,  32678,  32692,  32705,  32717,
	32728,  32737,  32745,  32752,  32757,  32761,  32765,  32766,
	32767,  32766,  32765,  32761,  32757,  32752,  32745,  32737,
	32728,  32717,  32705,  32692,  32678,  32663,  32646,  32628,
	32609,  32589,  32567,  32545,  32521,  32495,  32469,  32441,
	32412,  32382,  32351,  32318,  32285,  32250,  32213,  32176,
	32137,  32098,  32057,  32014,  31971,  31926,  31880,  31833,
	31785,  31736,  31685,  31633,  31580,  31526,  31470,  31414,
	31356,  31297,  31237,  31176,  31113,  31050,  30985,  30919,
	30852,  30783,  30714,  30643,  30571,  30498,  30424,  30349,
	30273,  30195,  30117,  30037,  29956,  29874,  29791,  29706,
	29621,  29534,  29447,  29358,  29268,  29177,  29085,  28992,
	28898,  28803,  28706,  28609,  28510,  28411,  28310,  28208,
	28105,  28001,  27896,  27790,  27683,  27575,  27466,  27356,
	27245,  27133,  27019,  26905,  26790,  26674,  26556,  26438,
	26319,  26198,  26077,  25955,  25832,  25708,  25582,  25456,
	25329,  25201,  25072,  24942,  24811,  24680,  24547,  24413,
	24279,  24143,  24007,  23870,  23731,  23592,  23452,  23311,
	23170,  23027,  22884,  22739,  22594,  22448,  22301,  22154,
	22005,  21856,  21705,  21554,  21403,  21250,  21096,  20942,
	20787,  20631,  20475,  20317,  20159,  20000,  19841,  19680,
	19519,  19357,  19195,  19032,  18868,  18703,  18537,  18371,
	18204,  18037,  17869,  17700,  17530,  17360,  17189,  17018,
	16846,  16673,  16499,  16325,  16151,  15976,  15800,  15623,
	15446,  15269,  15090,  14912,  14732,  14553,  14372,  14191,
	14010,  13828,  13645,  13462,  13279,  13094,  12910,  12725,
	12539,  12353,  12167,  11980,  11793,  11605,  11417,  11228,
	11039,  10849,  10659,  10469,  10278,  10087,   9896,   9704,
	9512,   9319,   9126,   8933,   8739,   8545,   8351,   8157,
	7962,   7767,   7571,   7375,   7179,   6983,   6786,   6590,
	6393,   6195,   5998,   5800,   5602,   5404,   5205,   5007,
	4808,   4609,   4410,   4210,   4011,   3811,   3612,   3412,
	3212,   3012,   2811,   2611,   2410,   2210,   2009,   1809,
	1608,   1407,   1206,   1005,    804,    603,    402,    201,
	0,      -201,   -402,   -603,   -804,  -1005,  -1206,  -1407,
	-1608,  -1809,  -2009,  -2210,  -2410,  -2611,  -2811,  -3012,
	-3212,  -3412,  -3612,  -3811,  -4011,  -4210,  -4410,  -4609,
	-4808,  -5007,  -5205,  -5404,  -5602,  -5800,  -5998,  -6195,
	-6393,  -6590,  -6786,  -6983,  -7179,  -7375,  -7571,  -7767,
	-7962,  -8157,  -8351,  -8545,  -8739,  -8933,  -9126,  -9319,
	-9512,  -9704,  -9896, -10087, -10278, -10469, -10659, -10849,
	-11039, -11228, -11417, -11605, -11793, -11980, -12167, -12353,
	-12539, -12725, -12910, -13094, -13279, -13462, -13645, -13828,
	-14010, -14191, -14372, -14553, -14732, -14912, -15090, -15269,
	-15446, -15623, -15800, -15976, -16151, -16325, -16499, -16673,
	-16846, -17018, -17189, -17360, -17530, -17700, -17869, -18037,
	-18204, -18371, -18537, -18703, -18868, -19032, -19195, -19357,
	-19519, -19680, -19841, -20000, -20159, -20317, -20475, -20631,
	-20787, -20942, -21096, -21250, -21403, -21554, -21705, -21856,
	-22005, -22154, -22301, -22448, -22594, -22739, -22884, -23027,
	-23170, -23311, -23452, -23592, -23731, -23870, -24007, -24143,
	-24279, -24413, -24547, -24680, -24811, -24942, -25072, -25201,
	-25329, -25456, -25582, -25708, -25832, -25955, -26077, -26198,
	-26319, -26438, -26556, -26674, -26790, -26905, -27019, -27133,
	-27245, -27356, -27466, -27575, -27683, -27790, -27896, -28001,
	-28105, -28208, -28310, -28411, -28510, -28609, -28706, -28803,
	-28898, -28992, -29085, -29177, -29268, -29358, -29447, -29534,
	-29621, -29706, -29791, -29874, -29956, -30037, -30117, -30195,
	-30273, -30349, -30424, -30498, -30571, -30643, -30714, -30783,
	-30852, -30919, -30985, -31050, -31113, -31176, -31237, -31297,
	-31356, -31414, -31470, -31526, -31580, -31633, -31685, -31736,
	-31785, -31833, -31880, -31926, -31971, -32014, -32057, -32098,
	-32137, -32176, -32213, -32250, -32285, -32318, -32351, -32382,
	-32412, -32441, -32469, -32495, -32521, -32545, -32567, -32589,
	-32609, -32628, -32646, -32663, -32678, -32692, -32705, -32717,
	-32728, -32737, -32745, -32752, -32757, -32761, -32765, -32766,
};

/******************************************************************************/
/* Look up Table for linear to dB conversion                                  */
/* k = msb                                                                    */
/* dB = 3k + delta                                                            */
/* delta up to 4 bits                                                         */
/*                                                                            */
/*      delta      value  (dB)                                                */
/*          0       0                                                         */
/*          1       0.2633                                                    */
/*          2       0.5115                                                    */
/*          3       0.7463                                                    */
/*          4       0.9691                                                    */
/*          5       1.1810                                                    */
/*          6       1.3830                                                    */
/*          7       1.5761                                                    */
/*          8       1.7609                                                    */
/*          9       1.9382                                                    */
/*         10       2.1085                                                    */
/*         11       2.2724                                                    */
/*         12       2.4304                                                    */
/*         13       2.5828                                                    */
/*         14       2.7300                                                    */
/*         15       2.8724                                                    */
/*                                                                            */
/******************************************************************************/
UINT16 MATH_dBLut[16] =
{
     0,
     4,
     8,
    12,
    16,
    19,
    22,
    25,
    28,
    31,
    34,
    36,
    39,
    41,
    44,
    46
}; 

/******************************************************************************/
/* Cosine table:                                                              */
/* The table contains consine values for angle range 0 to 90 degree           */
/* in increment of (90/64) degrees. In the table, floating point 1.0 is       */
/* represented by the fixed point value 4096 or 0x1000 (12 fractional bits).  */
/******************************************************************************/
const SINT16 MATH_cosLut[65] =
{
  4096, 4095, 4091, 4085, 4076, 4065, 4052, 4036,
  4017, 3996, 3973, 3948, 3920, 3889, 3857, 3822,
  3784, 3745, 3703, 3659, 3612, 3564, 3513, 3461,
  3406, 3349, 3290, 3229, 3166, 3102, 3035, 2967,
  2896, 2824, 2751, 2675, 2598, 2520, 2440, 2359,
  2276, 2191, 2106, 2019, 1931, 1842, 1751, 1660,
  1567, 1474, 1380, 1285, 1189, 1092,  995,  897,
   799,  700,  601,  501,  401,  301,  201,  101,   0
};

static const uint16 crc8_table[256] = 
{
	0x0000, 0x0007, 0x000E, 0x0009, 0x001C, 0x001B, 0x0012, 0x0015,
	0x0038, 0x003F, 0x0036, 0x0031, 0x0024, 0x0023, 0x002A, 0x002D,
	0x0070, 0x0077, 0x007E, 0x0079, 0x006C, 0x006B, 0x0062, 0x0065,
	0x0048, 0x004F, 0x0046, 0x0041, 0x0054, 0x0053, 0x005A, 0x005D,
	0x00E0, 0x00E7, 0x00EE, 0x00E9, 0x00FC, 0x00FB, 0x00F2, 0x00F5,
	0x00D8, 0x00DF, 0x00D6, 0x00D1, 0x00C4, 0x00C3, 0x00CA, 0x00CD,
	0x0090, 0x0097, 0x009E, 0x0099, 0x008C, 0x008B, 0x0082, 0x0085,
	0x00A8, 0x00AF, 0x00A6, 0x00A1, 0x00B4, 0x00B3, 0x00BA, 0x00BD,
	0x00C7, 0x00C0, 0x00C9, 0x00CE, 0x00DB, 0x00DC, 0x00D5, 0x00D2,
	0x00FF, 0x00F8, 0x00F1, 0x00F6, 0x00E3, 0x00E4, 0x00ED, 0x00EA,
	0x00B7, 0x00B0, 0x00B9, 0x00BE, 0x00AB, 0x00AC, 0x00A5, 0x00A2,
	0x008F, 0x0088, 0x0081, 0x0086, 0x0093, 0x0094, 0x009D, 0x009A,
	0x0027, 0x0020, 0x0029, 0x002E, 0x003B, 0x003C, 0x0035, 0x0032,
	0x001F, 0x0018, 0x0011, 0x0016, 0x0003, 0x0004, 0x000D, 0x000A,
	0x0057, 0x0050, 0x0059, 0x005E, 0x004B, 0x004C, 0x0045, 0x0042,
	0x006F, 0x0068, 0x0061, 0x0066, 0x0073, 0x0074, 0x007D, 0x007A,
	0x0089, 0x008E, 0x0087, 0x0080, 0x0095, 0x0092, 0x009B, 0x009C,
	0x00B1, 0x00B6, 0x00BF, 0x00B8, 0x00AD, 0x00AA, 0x00A3, 0x00A4,
	0x00F9, 0x00FE, 0x00F7, 0x00F0, 0x00E5, 0x00E2, 0x00EB, 0x00EC,
	0x00C1, 0x00C6, 0x00CF, 0x00C8, 0x00DD, 0x00DA, 0x00D3, 0x00D4,
	0x0069, 0x006E, 0x0067, 0x0060, 0x0075, 0x0072, 0x007B, 0x007C,
	0x0051, 0x0056, 0x005F, 0x0058, 0x004D, 0x004A, 0x0043, 0x0044,
	0x0019, 0x001E, 0x0017, 0x0010, 0x0005, 0x0002, 0x000B, 0x000C,
	0x0021, 0x0026, 0x002F, 0x0028, 0x003D, 0x003A, 0x0033, 0x0034,
	0x004E, 0x0049, 0x0040, 0x0047, 0x0052, 0x0055, 0x005C, 0x005B,
	0x0076, 0x0071, 0x0078, 0x007F, 0x006A, 0x006D, 0x0064, 0x0063,
	0x003E, 0x0039, 0x0030, 0x0037, 0x0022, 0x0025, 0x002C, 0x002B,
	0x0006, 0x0001, 0x0008, 0x000F, 0x001A, 0x001D, 0x0014, 0x0013,
	0x00AE, 0x00A9, 0x00A0, 0x00A7, 0x00B2, 0x00B5, 0x00BC, 0x00BB,
	0x0096, 0x0091, 0x0098, 0x009F, 0x008A, 0x008D, 0x0084, 0x0083,
	0x00DE, 0x00D9, 0x00D0, 0x00D7, 0x00C2, 0x00C5, 0x00CC, 0x00CB,
	0x00E6, 0x00E1, 0x00E8, 0x00EF, 0x00FA, 0x00FD, 0x00F4, 0x00F3,
};

/*===========================================================================
  Function   : getCRC8
  Description :
    This function is to calculate the CRC8 from a linear bufffer

  Parameters:
  intput :
      input_crc8_accum: the init value of crc, in case the message has been
          chopped into several parts, you can use the crc8 of the previsou
          segment as the init value for the current segment crc8 calculation
          until the final crc is derived.
      msg         : pointer of the word stream of the message
      offset      : ODD or EVEN to tell the starting byte in the word stream
      rxLen       : number of BYTES of the input msg
  output : none
  return : crc value
===========================================================================*/
uint16 getCRC8 (uint16 input_crc8_accum, uint16 * msg, parity_t parity, uint16 rxLen)
{
	uint16 i, j;

	uint16 crc8_accum = input_crc8_accum;
	SINT16 *pdata = (SINT16 *)msg;

	for (j = 0; j < rxLen; j++, parity++)
	{
		i = crc8_accum ^ pdata[parity];

		crc8_accum = crc8_table[i]; 
	}

	return crc8_accum;
}


char *PhyTxStr[] = 
{
	"TX_IDLE",
	"TX_START",
	"TX_SYNCP",
	"TX_SYNCM",
	"TX_HDR",
	"TX_DATA",
	"TX_LAST",
	"TX_FAIL"
};

char *EventStr[] = 
{
	"TX_EVT_START",
	"TX_EVT_RETRY",
	"TX_EVT_FAIL",
	"TX_EVT_DONE",
};

char *PhyRxTopStr[] = 
{
	"RX_IDLE",
	"RX_IACQ",
	"RX_LSTN",
	"RX_HOLD",
	"RX_SCHD",
};

char *EventRxTopStr[] = 
{
	"RX_EVT_READY",
	"RX_EVT_START",
	"RX_EVT_DONE",
	"RX_EVT_SCHD",
	"RX_EVT_STOP",
	"RX_EVT_FAIL",
};

char *PhyRxSsmStr[] = 
{
	"SSM_HUNT",
	"SSM_VFY",
	"SSM_SYNC",
	"SSM_ALGN",
	"SSM_HDR2",
	"SSM_STDY",
};

char *EventRxSsmStr[] = 
{
	"SSM_EVT_READY",
	"SSM_EVT_DONE",
	"SSM_EVT_HDR_DONE",
	"SSM_EVT_ERR",
};

/******************************************************************************
* FUNCTION NAME: FSM_event
*
* DESCRIPTION:   This function perform event transition in the matrix
* 
* Return Value:  
*
* Input Parameters:  fsm_p - pointer to the state machine structure
*                    event - event causing transition 
*                    data_p - data for activation function 
*
* Output Parameters: update current state after action taken
* Functions Called:  
* 
* RLiang:
* we may need to add the mutex code when update FSM current state
******************************************************************************/
PRIME_status_t FSM_event(FSM_stateMachine_t *fsm_p, UINT16 event, void *data_p)
{
	UINT32         idx;
	PRIME_status_t status;

	if ((fsm_p->currentState >= fsm_p->maxStates) || 
  	    (event >= fsm_p->maxEvents))
    {
		return PRIME_STAT_ERROR;
    }

	if (fsm_p->transitionFunc_p)
	{  
		(fsm_p->transitionFunc_p)(fsm_p->currentState, event, data_p);
	}
  
	fsm_p->previousState = fsm_p->currentState;

	idx = (fsm_p->currentState * fsm_p->maxEvents) + event;

	fsm_p->currentState = fsm_p->stateEventMatrix[idx].nextState;
	
	if (fsm_p == &PHY_txSm)
	{
		PLC_print("[FSM_event]: curState = %s, event = %s, nextState = %s\r\n", PhyTxStr[fsm_p->previousState], EventStr[event], PhyTxStr[fsm_p->currentState]);
	}
	else if (fsm_p == &PHY_rxSmTop)
	{
		PLC_print("[FSM_event]: curState = %s, event = %s, nextState = %s\r\n", PhyRxTopStr[fsm_p->previousState], EventRxTopStr[event], PhyRxTopStr[fsm_p->currentState]);
	}
	else if ( (fsm_p == &PHY_rxSsmIacq) || (fsm_p == &PHY_rxSsmLstn) || (fsm_p == &PHY_rxSsmSchd) )
	{
		PLC_print("[FSM_event]: curState = %s, event = %s, nextState = %s\r\n", PhyRxSsmStr[fsm_p->previousState], EventRxSsmStr[event], PhyRxSsmStr[fsm_p->currentState]);
	}
	else
	{
	}

	status = (*fsm_p->stateEventMatrix[idx].actionFunc_p)(data_p);

	return status;
}

/******************************************************************************
* FUNCTION NAME: FSM_getNextState
*
* DESCRIPTION:   This function returns the next for a given current state and event
* 
* Return Value:  
*
* Input Parameters:  fsm_p - pointer to the state machine structure
*                    event - event causing transition 
*                    nextState - returned next state
*
* Output Parameters: 
* Functions Called:  
*
******************************************************************************/
PRIME_status_t FSM_getNextState(FSM_stateMachine_t *fsm_p, UINT16 event, UINT16 *nextState_p)
{
	if ((fsm_p->currentState < fsm_p->maxStates) && (event < fsm_p->maxEvents))
    {
		*nextState_p = fsm_p->stateEventMatrix[(fsm_p->currentState * fsm_p->maxEvents) + event].nextState;

		return(PRIME_STAT_SUCCESS);
    }

	return PRIME_STAT_ERROR;
}


/******************************************************************************
* FUNCTION NAME: PHY_aagcInit
*
* DESCRIPTION:   This function initializes AAGC measurement block size.
*
* Return Value:       
*
* Input Parameters:   aagc_p - pointer aagc structure
*                     stage  - PHY_AAGC_STAGE_IACQ  (initial sync)
*                              PHY_AAGC_STAGE_STDY  (steady)
*
* Output Parameters:  
*
******************************************************************************/
void PHY_aagcInit(PHY_aagc_t *aagc_p, PHY_aagcStage_t stage)
{
	/* Setup blk size */
	if (stage == PHY_AAGC_STAGE_IACQ)
    {
		aagc_p->tblkLen = 1024;
		aagc_p->dblkLen = 64;
		aagc_p->tShift  = 8;
		aagc_p->dShift  = 5;
    }
	else
    {
		aagc_p->tblkLen = 1024;
		aagc_p->dblkLen = 64;
		aagc_p->tShift  = 8;
		aagc_p->dShift  = 5;
    }
}

SINT32 __IQsat(SINT32 llrTmp, SINT32 max, SINT32 min)
{
	SINT32 Temp = llrTmp;

	if (Temp > max)  Temp = max;
	if (Temp < min)  Temp = min;

	return Temp;
}

/******************************************************************************
* FUNCTION NAME: PHY_aagcQuantizeGain
*
* DESCRIPTION:   This function quanizes to PGA gain Idx
*
* Return Value:       
*
* Input Parameters:    
*                      gain_dB  - desired gain in dB to be quantized to PGA 
*                                 step.
*
* Output Parameters:  
*
******************************************************************************/
UINT16 PHY_aagcQuantizeGain(SINT32 gain_dB)
{
	SINT16 pgaIdx, nextPga_dB;

	/* limit the gain to PGA range */
	gain_dB = __IQsat(gain_dB, 42, 0);

	/* Quantize to gain setting, in terms of PGA112 gain step */
	pgaIdx = (gain_dB * 21) >> 7;

	/* check if next step is ok */
	nextPga_dB = (pgaIdx + 1) * 6;

	if ((pgaIdx < 7) && (gain_dB >= nextPga_dB))
	{
		pgaIdx++;
	}

	return pgaIdx;
}

/******************************************************************************
* FUNCTION NAME: PHY_aagcSet
*
* DESCRIPTION:   This function sets AAGC to start with Alg A/B/C/D.
*
* Return Value:       
*
* Input Parameters:    mode   - select alg A/B/C/D 
*                      idx    - If mode A, then initial gain (only if alg A)
*                                             (0 = -4 dB, 1 = 0 dB, 2 = 12 dB)
*                               If mode B, node idx (0/1 for now)
*                      aagc_p - pointer aagc structure
*
* Output Parameters:  aagc_p->gain
*
******************************************************************************/
void PHY_aagcSet(PHY_aagcMode_t mode, SINT16 idx, PHY_aagc_t *aagc_p)
{
	SINT16 prvIdx; 
	SINT32 gain_dB;  // intrinsics only takes long value

	prvIdx  = aagc_p->currIdx ^ 0x1;

	/* Set up for target power */
	aagc_p->mode        = mode;
	aagc_p->tgtPower_dB = PHY_aagcTgtPower[mode];

	/* setup for initial gain */
	switch (mode)
    {
    case PHY_AAGC_MODE_A:
        /* PT, Pd, Paci unknown, fixed gain setting */
        /* limit gain index */
        if (idx >= 3)    idx = 2;

        /* Quantize to gain setting, in terms of PGA112 gain step */
        aagc_p->initGainIdx = PHY_aagcAlgAGainIdx[idx];
        break;

    case PHY_AAGC_MODE_B:
        /* Compute gain g(n+1) = Ptgt - Ptot(node from prv burst) + gain(n) */
        gain_dB = aagc_p->tgtPower_dB - aagc_p->measNodes[idx].totPower_dB + aagc_p->meas[prvIdx].gainIdx * 6;

        aagc_p->initGainIdx = PHY_aagcQuantizeGain(gain_dB);        

        break;

    case PHY_AAGC_MODE_C: 
        gain_dB = aagc_p->tgtPower_dB - aagc_p->meas[prvIdx].aciPower_dB + aagc_p->meas[prvIdx].gainIdx * 6;

        /* Quantize to gain setting, in terms of PGA112 gain step */
        if (gain_dB > 24)   gain_dB = 24;

        aagc_p->initGainIdx = PHY_aagcQuantizeGain(gain_dB);        

        break;

    case PHY_AAGC_MODE_D: 
        break;

    default:
		break;
    }

	/* Apply gain */
	if (PHY_rxDbg_s.aagcBypass)
	{
		aagc_p->initGainIdx = PHY_rxDbg_s.aagcFixGain;
	}

	HAL_afeSet(AFE_RX_UPDATEGAIN, &aagc_p->initGainIdx);  
}

/******************************************************************************
* FUNCTION NAME: PHY_aagcMeasure
*
* DESCRIPTION:   This function computes the power of the real input signal
*                from ADC, desired signal power and ACI power.
*
* Return Value:       
*
* Input Parameters:    
*                      adc_p  - pointer to adc o/p         (Q1.11)
*                      dfe_p  - pointer to dfe o/p         (Q1.11)
*                      aagc_p  - pointer to aagc struct
*
* Output Parameters:  
*
******************************************************************************/
void PHY_aagcMeasure(UINT32 aagcPower, ComplexShort *dfe_p, PHY_aagc_t *aagc_p)
{
	PHY_aagcMeas_t *meas_p = &aagc_p->meas[aagc_p->currIdx];                      

	/* Compute total power from ADC output */
	SINT32 power = aagcPower >> aagc_p->tShift;

	meas_p->totPower_dB = (SINT16)MATH_Q0linearToQ4dB(power) >> 4;

	/* Compute desired power */
	power = UTIL_blockShortComplexPower(aagc_p->dblkLen, (SINT16 *)dfe_p) >> aagc_p->dShift;

	meas_p->dsrPower_dB = (SINT16)MATH_Q0linearToQ4dB(power) >> 4;

	/* Compute ACI */
	meas_p->aciPower_dB = meas_p->totPower_dB - meas_p->dsrPower_dB;
}

/******************************************************************************
* FUNCTION NAME: PHY_aagcUpdateGain
*
* DESCRIPTION:   This function computes the gain based on target power
*
* Return Value:       
*
* Input Parameters:    
*                      aagc_p  - pointer to aagc struct
*
* Output Parameters:  
*
******************************************************************************/
void PHY_aagcUpdateGain(PHY_aagc_t *aagc_p)
{
	SINT16 currIdx, prvIdx;
	SINT32 gain_dB;  

	currIdx = aagc_p->currIdx;
	prvIdx  = aagc_p->currIdx ^ 0x1;

	/* Compute gain g(n+1) = Ptgt - Ptot + gain(n) */
	gain_dB = aagc_p->tgtPower_dB - aagc_p->meas[currIdx].totPower_dB + aagc_p->meas[prvIdx].gainIdx * 6 ;

	/* Quantize to gain setting, in terms of PGA112 gain step */
	aagc_p->stdyGainIdx = PHY_aagcQuantizeGain(gain_dB);

	/* Call afe device driver to update gain, don't apply yet, only @ CP */
	if (PHY_rxDbg_s.aagcBypass)
    {
		aagc_p->stdyGainIdx = PHY_rxDbg_s.aagcFixGain;
    }

	HAL_afeSet(AFE_RX_UPDATEGAIN, &aagc_p->stdyGainIdx);  
}

/******************************************************************************
* FUNCTION NAME: PHY_aagcUpdateMeas
*
* DESCRIPTION:   This function updates measurements and gains
*
* Return Value:       
*
* Input Parameters:    
*                      aagc_p  - pointer to aagc struct
*
* Output Parameters:  
*
******************************************************************************/
void PHY_aagcUpdateMeas(PHY_aagc_t *aagc_p)
{
	aagc_p->currIdx ^= 0x1;
}

/******************************************************************************
* FUNCTION NAME: PHY_firInit
*
* DESCRIPTION:   This function initializes the FIR structure and reset delay 
*                line. This is for even number of filter taps.
*
*
* Input Parameters:    fir_p   - pointer to FIR structure
*                      coeff_p - pointer to filter coeff
*                      nTaps   - number of filter taps
*
* Output Parameters:   
*
******************************************************************************/
void PHY_firInit(PHY_fir_t *fir_p, const SINT16  *coeff_p, UINT16 nTaps)
{
	fir_p->coeff_p = coeff_p;
	fir_p->nTaps   = nTaps;
}

/******************************************************************************
* FUNCTION NAME: PHY_cfft16_brev
*
* Description   : This function shuffles N-point complex in-order array and stores 
*                 it as N-point complex array in bit-reversed order. 
*
* Return Value:
*
*  Parameters:
*       fft_hnd -- FFT handle
******************************************************************************/
void PHY_cfft16_brev(PHY_cfft16_t *fft_hnd)
{
    SINT16 i, j = 0;
	
	ComplexShort *s1Ptr = (ComplexShort *)fft_hnd->ipcbptr;

    for (i = 1; i < fft_hnd->size; i++)
    {
        SINT16 k = fft_hnd->size / 2;

        while (k <= j)
        {
            j -= k;
            k >>= 1;
        }

        j += k;

        if (i < j)
        {
            SINT16 temp1 = s1Ptr[j].real;
            SINT16 temp2 = s1Ptr[j].imag;

            s1Ptr[j].real = s1Ptr[i].real;
            s1Ptr[j].imag = s1Ptr[i].imag;

            s1Ptr[i].real = temp1;
            s1Ptr[i].imag = temp2;
        }
    }  
}

/******************************************************************************
* FUNCTION NAME: PHY_cfft16
*
* DESCRIPTION:   This function does initialization for complex 16-bit FFT.
*
* Return Value:
*
* Input Parameters:
*
* Output Parameters:
*
******************************************************************************/
void PHY_cfft16(PHY_cfft16_t *fft_hnd)
{
	PHY_cfft16_brev(fft_hnd);

	fft_hnd->calc(fft_hnd);
}

/******************************************************************************
* FUNCTION NAME: PHY_cfft16_init
*
* DESCRIPTION:   This function does initialization for complex 16-bit FFT.
*
* Return Value:
*
* Input Parameters:
*
* Output Parameters:
*
******************************************************************************/
void PHY_cfft16_init(void *data_p)
{
	PHY_cfft16_t *fft_hnd = (PHY_cfft16_t *)data_p;

	fft_hnd->tfptr = (SINT16 *)cfft16_table;
}

/******************************************************************************
* FUNCTION NAME: PHY_cfft16_calc
*
* DESCRIPTION:   This function implements complex FFT using radix-2 DIT algo
*
* Return Value:
*
* Input Parameters:
*
* Output Parameters:
*
******************************************************************************/
void PHY_cfft16_calc(void *data_p)
{
	int i, j, k;
	SINT16 *sinPtr, *cosPtr;
	long roundoff = (1 << 15);

	PHY_cfft16_t *fft_hnd = (PHY_cfft16_t *)data_p;

	SINT16 weight = 23170;
    SINT16 butterflyCnt = 8;
    SINT16 groupCnt = fft_hnd->size / 16;
    SINT16 twiddleOffset = fft_hnd->step * fft_hnd->size / 4;
    SINT16 dataOffset = 8;

	// Stage 1 & 2 - Using the RADIX 4 COMBO Macro
	ComplexShort *s1Ptr = (ComplexShort *)fft_hnd->ipcbptr; 
	ComplexShort *s2Ptr = s1Ptr + 1; 
	ComplexShort *s3Ptr = s1Ptr + 2; 
	ComplexShort *s4Ptr = s1Ptr + 3; 

	for (i = 0; i < fft_hnd->size/4; i++)
	{
		SINT32 real1 = s1Ptr->real;
		SINT32 real2 = s2Ptr->real;
		SINT32 real3 = s3Ptr->real;
		SINT32 real4 = s4Ptr->real;
		SINT32 imag1 = s1Ptr->imag;
		SINT32 imag2 = s2Ptr->imag;
		SINT32 imag3 = s3Ptr->imag;
		SINT32 imag4 = s4Ptr->imag;

		s1Ptr->real = (SINT16)((real1 + real2 + real3 + real4) / 4);
		s2Ptr->real = (SINT16)((real1 - real2 + imag3 - imag4) / 4);
		s3Ptr->real = (SINT16)((real1 + real2 - real3 - real4) / 4);
		s4Ptr->real = (SINT16)((real1 - real2 - imag3 + imag4) / 4);
		s1Ptr->imag = (SINT16)((imag1 + imag2 + imag3 + imag4) / 4);
		s2Ptr->imag = (SINT16)((imag1 - imag2 - real3 + real4) / 4);
		s3Ptr->imag = (SINT16)((imag1 + imag2 - imag3 - imag4) / 4);
		s4Ptr->imag = (SINT16)((imag1 - imag2 + real3 - real4) / 4);

		s1Ptr += 4;
		s2Ptr += 4;
		s3Ptr += 4;
		s4Ptr += 4;
	}

	// Stage 3 - Using ZEROI, PBY4I,PBY2I,P3BY4I Macros
	s1Ptr = (ComplexShort *)fft_hnd->ipcbptr;
	s2Ptr = s1Ptr + 4;

	for (i = 0; i < fft_hnd->size/8; i++)
	{
		SINT32 temp;
		SINT32 real1 = s1Ptr->real;
		SINT32 real2 = s2Ptr->real;
		SINT32 imag1 = s1Ptr->imag;
		SINT32 imag2 = s2Ptr->imag;

		s1Ptr->real = (SINT16)((real1 + real2) / 2);
		s2Ptr->real = (SINT16)((real1 - real2) / 2);
		s1Ptr->imag = (SINT16)((imag1 + imag2) / 2);
		s2Ptr->imag = (SINT16)((imag1 - imag2) / 2);

		s1Ptr ++;
		s2Ptr ++;

		real1 = s1Ptr->real;
		real2 = s2Ptr->real;
		imag1 = s1Ptr->imag;
		imag2 = s2Ptr->imag;
	
		temp = (SINT16)((imag2 * weight + real2 * weight + roundoff) >> 16);
		s1Ptr->real = (SINT16)(real1 / 2 + temp);
		s2Ptr->real = (SINT16)(real1 / 2 - temp);

		temp = (SINT16)((imag2 * weight - real2 * weight + roundoff) >> 16);
		s1Ptr->imag = (SINT16)(imag1 / 2 + temp);
		s2Ptr->imag = (SINT16)(imag1 / 2 - temp);

		s1Ptr++;
		s2Ptr++;

		real1 = s1Ptr->real;
		real2 = s2Ptr->real;
		imag1 = s1Ptr->imag;
		imag2 = s2Ptr->imag;

		s1Ptr->real = (real1 + imag2) / 2; 
		s2Ptr->real = (real1 - imag2) / 2;
		s1Ptr->imag = (imag1 - real2) / 2;
		s2Ptr->imag = (imag1 + real2) / 2;

		s1Ptr++;
		s2Ptr++;

		real1 = s1Ptr->real;
		real2 = s2Ptr->real;
		imag1 = s1Ptr->imag;
		imag2 = s2Ptr->imag;

		temp = (SINT16)((imag2 * weight - real2 * weight + roundoff) >> 16);
		s1Ptr->real = (SINT16)(real1 / 2 + temp);
		s2Ptr->real = (SINT16)(real1 / 2 - temp);

		temp = (SINT16)((imag2 * weight + real2 * weight + roundoff) >> 16);
		s1Ptr->imag = (SINT16)(imag1 / 2 - temp);
		s2Ptr->imag = (SINT16)(imag1 / 2 + temp);

		s1Ptr++;
		s2Ptr++;
	}

	// Stage 4 to LOG2(N) - Using BFLY Macro
	for (i = 0; i < (fft_hnd->nrstage-3); i++)
	{
		s1Ptr = (ComplexShort *)fft_hnd->ipcbptr;
		s2Ptr = (ComplexShort *)(fft_hnd->ipcbptr + dataOffset);

		for (j = 0; j < groupCnt; j++)
		{
			sinPtr = fft_hnd->tfptr;
			cosPtr = fft_hnd->tfptr + twiddleOffset;

			for (k = 0; k < butterflyCnt; k++)
			{
				SINT32 temp;
				SINT32 WR = *cosPtr;
				SINT32 WI = *sinPtr;
				SINT32 real1 = s1Ptr->real;
				SINT32 real2 = s2Ptr->real;
				SINT32 imag1 = s1Ptr->imag;
				SINT32 imag2 = s2Ptr->imag;

				temp = (SINT16)((imag2 * weight + real2 * weight + roundoff) >> 16);
				s1Ptr->real = (SINT16)(real1 / 2 + temp);
				s2Ptr->real = (SINT16)(real1 / 2 - temp);

				temp = (SINT16)((imag2 * weight - real2 * weight + roundoff) >> 16);
				s1Ptr->imag = (SINT16)(imag1 / 2 + temp);
				s2Ptr->imag = (SINT16)(imag1 / 2 - temp);
				
				s1Ptr++;
				s2Ptr++;

				sinPtr += (twiddleOffset/4);
				cosPtr += (twiddleOffset/4);
			}

			s1Ptr += dataOffset;
			s2Ptr += dataOffset;
		}
            
        butterflyCnt *= 2;
        groupCnt /= 2;
        twiddleOffset /= 2;
		dataOffset *= 2;
	}
}

/******************************************************************************
* FUNCTION NAME: PHY_cfft16_128p_init
*
* DESCRIPTION:   This function initializes for 128 pt complex FFT
*
* Return Value:
*
* Input Parameters:
* Output Parameters:
*
******************************************************************************/
void PHY_cfft16_128p_init(PHY_cfft16_t *fft_p, ComplexShort *in_p)
{
	fft_p->ipcbptr = (SINT16 *)in_p;
	fft_p->size    = 128;
	fft_p->nrstage = 7;
	fft_p->step    = 8;
	fft_p->init    = PHY_cfft16_init;
	fft_p->calc    = PHY_cfft16_calc;
	fft_p->init(fft_p);
}

/******************************************************************************
* FUNCTION NAME: PHY_cfft16_flip_re_img
*
* Description   : flip real and imaginary part 
*
* Return Value:
*
*  Parameters:
*       fft_hnd -- FFT handle
******************************************************************************/
void PHY_cfft16_flip_re_img(PHY_cfft16_t *fft_hnd)
{
	int i;

	ComplexShort *srcPtr = (ComplexShort *)fft_hnd->ipcbptr;

	for (i = 0; i < fft_hnd->size; i++)
	{
		SINT16  imag = srcPtr->imag;
		srcPtr->imag = srcPtr->real;
		srcPtr->real = imag;

		srcPtr++;
	}
}

/******************************************************************************
* FUNCTION NAME: PHY_cifft16
*
* DESCRIPTION:   This function computes complex IFFT
*
* Return Value:
*
*  Parameters:
*       fft_hnd -- FFT handle
*
******************************************************************************/
void PHY_cifft16(PHY_cfft16_t *fft_hnd)
{
	/* Pre-process */
	/* Swap real and imaginary of input */
	PHY_cfft16_flip_re_img(fft_hnd);

	PHY_cfft16_brev(fft_hnd);

	fft_hnd->calc(fft_hnd);

	/* Post-process */
	/* Swap real and imaginary of input */
	PHY_cfft16_flip_re_img(fft_hnd);
}

/******************************************************************************
* FUNCTION NAME: PHY_dagc
*
* DESCRIPTION:   This function computes the power of the complex input signal
*                and scale the signal to target power.
*
* Return Value:       
*
* Input Parameters:    nSamp - number of complex sample 
*                      in_p  - pointer to input complex signal      (Q5.7)
*                      dagc_p - pointer to dagc structure (see phy_agc.h)
*
* Output Parameters:   dagc_p->power
*                      dagc_p->hdRoom
*                      in_p scaled to meet target power
*
******************************************************************************/
void PHY_dagc(SINT16 nSamps, ComplexShort *in_p, PHY_dagc_t *dagc_p)
{
	int i;

	SINT32 ltmp;
	SINT16 *data_p = (SINT16 *)in_p;

	/* Compute power */
	SINT32 power = UTIL_blockShortComplexPower(nSamps, (SINT16 *)in_p) >> 6;

	dagc_p->power_Q4dB = (SINT16)MATH_Q0linearToQ4dB(power);

	/* Compute gain = Target_rms - measured_rms */
	dagc_p->hdRoom_Q4dB = (SINT16)((1248 - dagc_p->power_Q4dB + 1)/2);

	ltmp = MATH_Q4dBToQ12linear(dagc_p->hdRoom_Q4dB) >> 12;   // Q0

	if (PHY_rxDbg_s.dagcFix)
	{
		ltmp = PHY_rxDbg_s.dagcGain;
	}

	/* scale data */
	if (ltmp < 1)   ltmp = 1;

	for (i = 0; i < (nSamps*2); i++)
	{
		data_p[i] = (SINT16)((long)data_p[i] * ltmp);
	}

	/* store it Q0 */
	dagc_p->gain[1] = (SINT16)ltmp;
}

/******************************************************************************
* FUNCTION NAME: PHY_cnvDecInit
*
* DESCRIPTION:   Hard-coded asm K=7, CR=1/2
*
* Input Parameters:    cnvEnc_p - pointer to cnvEnc structure
*
* Output Parameters:  
******************************************************************************/
void PHY_cnvDecInit(int nBits)
{
	int i;

	old_metric[0] = 0;

	for(i = 1; i < 16; i++)
	{
		old_metric[i] = (SINT16)0xC000;
	}
    
	trn_s1_p = &tran_hist[0];
	trn_s2_p = &tran_hist[nBits/2];
	trn_w1_p = &tran_hist[nBits/2];
	trn_w2_p = &tran_hist[nBits];
}

void PHY_cnvDecMetricRescale(void)
{
	int i;

	SINT16 min = 0x7FFF;

	for(i = 0; i < 16; i++)
	{
		if (old_metric[i] < min)
		{
			min = old_metric[i];
		}
	}

	for(i = 0; i < 16; i++)
	{
		old_metric[i] = min;
	}
}

/******************************************************************************
* FUNCTION NAME: PHY_cnvDec
*
* DESCRIPTION:   This function pack bits to 16-bits word
*
* Input Parameters:    nBits   - number of bits (multiple of 16-bits)
*                      offset  - byte offset for output
*                      in_p    - pointer to input data in bits
*                      
*
* Output Parameters:   out_p   - pointer to output data in 16-bits word           
*
******************************************************************************/
void PHY_cnvDec(UINT16 bit_in, SINT16 *in_p, UINT16 *out_p, int flag)
{
	int i, j, loop, overlap;

	SINT16 val1, val2, val3, out_bit;
    SINT16 *temp, *tran_save, *out_save;
    SINT16 *pold = old_metric, *pnew = new_metric;

	if (flag < 2)
	{
		tran_save = tran_hist;
	}
	else
	{
		tran_save = trn_s1_p;
	}

	for(i = 0; i < bit_in/2; i++)
	{
		SINT16 m_s, m_d, orsum = 0;

		for(j = 0; j < 16; j++)
		{
			trn_tmp[j] = 0;
		}
		
		m_s = in_p[2*i+0] + in_p[2*i+1];
		m_d = in_p[2*i+0] - in_p[2*i+1];

		val1 = pold[0] + m_s;
		val2 = pold[1] - m_s;
		if(val1 > val2)
		{
		    val3 = val1;
		}
		else
		{
			val3 = val2;
		    trn_tmp[0] = 1;
		}
		pnew[0] = val3;

		val1 = pold[0] - m_s;
		val2 = pold[1] + m_s;
		if(val1 > val2)
		{
		    val3 = val1;
		}
		else
		{
			val3 = val2;
		    trn_tmp[8] = 1;
		}
		pnew[8] = val3;

		val1 = pold[2] - m_s;
		val2 = pold[3] + m_s;
		if(val1 > val2)
		{
		    val3 = val1;
		}
		else
		{
			val3 = val2;
		    trn_tmp[1] = 2;
		}
		pnew[1] = val3;

		val1 = pold[2] + m_s;
		val2 = pold[3] - m_s;
		if(val1 > val2)
		{
		    val3 = val1;
		}
		else
		{
			val3 = val2;
		    trn_tmp[9] = 2;
		}
		pnew[9] = val3;

		val1 = pold[4] + m_s;
		val2 = pold[5] - m_s;
		if(val1 > val2)
		{
		    val3 = val1;
		}
		else
		{
			val3 = val2;
		    trn_tmp[2] = 4;
		}
		pnew[2] = val3;

		val1 = pold[4] - m_s;
		val2 = pold[5] + m_s;
		if(val1 > val2)
		{
		    val3 = val1;
		}
		else
		{
			val3 = val2;
		    trn_tmp[10] = 4;
		}
		pnew[10] = val3;

		val1 = pold[6] - m_s;
		val2 = pold[7] + m_s;
		if(val1 > val2)
		{
		    val3 = val1;
		}
		else
		{
			val3 = val2;
		    trn_tmp[3] = 8;
		}
		pnew[3] = val3;

		val1 = pold[6] + m_s;
		val2 = pold[7] - m_s;
		if(val1 > val2)
		{
		    val3 = val1;
		}
		else
		{
			val3 = val2;
		    trn_tmp[11] = 8;
		}
		pnew[11] = val3;

		val1 = pold[8] + m_d;
		val2 = pold[9] - m_d;
		if(val1 > val2)
		{
		    val3 = val1;
		}
		else
		{
			val3 = val2;
		    trn_tmp[4] = 16;
		}
		pnew[4] = val3;

		val1 = pold[8] - m_d;
		val2 = pold[9] + m_d;
		if(val1 > val2)
		{
		    val3 = val1;
		}
		else
		{
			val3 = val2;
		    trn_tmp[12] = 16;
		}
		pnew[12] = val3;

		val1 = pold[10] - m_d;
		val2 = pold[11] + m_d;
		if(val1 > val2)
		{
		    val3 = val1;
		}
		else
		{
			val3 = val2;
		    trn_tmp[5] = 32;
		}
		pnew[5] = val3;

		val1 = pold[10] + m_d;
		val2 = pold[11] - m_d;
		if(val1 > val2)
		{
		    val3 = val1;
		}
		else
		{
			val3 = val2;
		    trn_tmp[13] = 32;
		}
		pnew[13] = val3;

		val1 = pold[12] + m_d;
		val2 = pold[13] - m_d;
		if(val1 > val2)
		{
		    val3 = val1;
		}
		else
		{
			val3 = val2;
		    trn_tmp[6] = 64;
		}
		pnew[6] = val3;

		val1 = pold[12] - m_d;
		val2 = pold[13] + m_d;
		if(val1 > val2)
		{
		    val3 = val1;
		}
		else
		{
			val3 = val2;
		    trn_tmp[14] = 64;
		}
		pnew[14] = val3;

		val1 = pold[14] - m_d;
		val2 = pold[15] + m_d;
		if(val1 > val2)
		{
		    val3 = val1;
		}
		else
		{
			val3 = val2;
		    trn_tmp[7] = 128;
		}
		pnew[7] = val3;

		val1 = pold[14] + m_d;
		val2 = pold[15] - m_d;
		if(val1 > val2)
		{
		    val3 = val1;
		}
		else
		{
			val3 = val2;
		    trn_tmp[15] = 128;
		}
		pnew[15] = val3;

		for(j = 8; j < 16; j++)
		{
			orsum |= trn_tmp[j];
		}
		orsum <<= 8;

		for(j = 0; j < 8; j++)
		{
			orsum |= trn_tmp[j];
		}

		*tran_save++ = orsum;

		temp = pold;
		pold = pnew;
		pnew = temp;
	}

	if (flag == 1) 
	{
		temp = trn_s1_p;
		trn_s1_p = trn_s2_p;
		trn_s2_p = temp;

		return;
	}

	out_bit = bit_in/2;

	for(i = 0; i < 4; i++)
	{
		trn_tmp[out_bit-4+i] = 0;
	}

	if (flag != 0)  
	{
		loop = out_bit;
		out_save = &trn_tmp[out_bit-4+64];
		overlap = 64 - 5;
	}
	else
	{
		loop = out_bit - 4;
		out_save = &trn_tmp[out_bit-4];
		overlap = -1;
	}

	j = 0;

	for(i = 0; i < loop; i++)
	{
		SINT16 b;
		int n = j & 15;

		--tran_save;
		--out_save;

		b = (*tran_save >> n) & 0x1;
		*out_save = b;

		j = ((j << 1) + b) & 15;
	}

	if (flag != 0)
	{
		tran_save = trn_w1_p;

		for(i = 0; i < overlap; i++)
		{
			SINT16 b;
			int n = j & 15;
		
			--tran_save;
			--out_save;

			b = (*(trn_w1_p - i) >> n) & 0x1;
			*out_save = b;

			j = ((j << 1) + b) & 15;
		}
	}

	temp = trn_w1_p;
	trn_w1_p = trn_w2_p;
	trn_w2_p = temp;

	temp = trn_s1_p;
	trn_s1_p = trn_s2_p;
	trn_s2_p = temp;
}
              
/******************************************************************************
* FUNCTION NAME: PHY_cnvEncInit
*
* DESCRIPTION:   This function inits the delays
*
* Input Parameters:    cnvEnc_p - pointer to cnvEnc structure
*
* Output Parameters:  
******************************************************************************/
void PHY_cnvEncInit(PHY_cnvEnc_t *cnvEnc_p)
{
	UTIL_blockShortFill(4, (SINT16 *)cnvEnc_p->delay, 0);
}

/******************************************************************************
* FUNCTION NAME: PHY_cnvEnc
*
* DESCRIPTION:   This function performs convolution encoding in prime. It uses
*                constraint length of K=7, and code rate 1/2.
*
*                The polynomials are:
*                G0 = 1 + D + D2 +D3 + D6                                            
*                G1 = 1 + D2 + D3 + D5 + D6                                          
*                                                                      
*                It calculates:
*                c(2k  )=u(k)+u(k-1)+u(k-2)+u(k-3)+u(k-6)                  
*                c(2k+1)=u(k)+u(k-2)+u(k-3)+u(k-5)+u(k-6)
*
* Input Parameters:    nBits - number of input bits for decode                         
*                      u_p   - pointer to input uncoded bits                                    
* Output Parameters:   c_p   - pointer to output coded bits 
*                      (It produces 2xnBit bits)                           
*
******************************************************************************/
void PHY_cnvEnc(PHY_cnvEnc_t *cnvEnc_p)
{
	int i;

	UINT16 *u_p = cnvEnc_p->u_p;
	UINT16 *uu_p = cnvEnc_p->u_p;
	UINT16 *c_p = cnvEnc_p->c_p;
	UINT16 *d_p = &cnvEnc_p->delay[0];

	c_p[0]  = (u_p[0] ^                    d_p[1] ^ d_p[0]); 
	c_p[1]  = (u_p[0] ^  d_p[3] ^          d_p[1] ^ d_p[0]); 
	c_p[2]  = (u_p[1] ^                    d_p[2] ^ d_p[1]); 
	c_p[3]  = (u_p[1] ^  u_p[0] ^          d_p[2] ^ d_p[1]); 
	c_p[4]  = (u_p[2] ^                    d_p[3] ^ d_p[2]); 
	c_p[5]  = (u_p[2] ^  u_p[1] ^          d_p[3] ^ d_p[2]); 
	c_p[6]  = (u_p[3] ^                    u_p[0] ^ d_p[3]); 
	c_p[7]  = (u_p[3] ^  u_p[2] ^          u_p[0] ^ d_p[3]); 
  
	c_p = &c_p[8];

	for(i = 4; i < cnvEnc_p->nBits; i++)
    {
		UINT16 tmp = uu_p[4] ^ uu_p[1] ^ uu_p[0];
		*c_p++ = tmp ;
		*c_p++ = tmp ^ uu_p[3];
		uu_p++;
    }
  
	/* Update delays */
	u_p += (cnvEnc_p->nBits - 4);

	for (i = 0; i < 4; i++)
	{
		*d_p++ = *u_p++; 
	}
}

/******************************************************************************
* FUNCTION NAME: PHY_scm_pnLoad
*
* DESCRIPTION:   Reload PN sequence for scrambler
*
* Return Value:  None
*
* Input Parameters:
*
* Output Parameters:
* Functions Called:
*
******************************************************************************/
void PHY_scm_pnLoad()
{
	PHY_scm_handle_s.pnScm_p = (Uint16 *)PHY_pref;
}

/******************************************************************************
* FUNCTION NAME: PHY_scm
*
* DESCRIPTION:   Generate scrambled bits for header & payload
*
* Return Value:  None
*
* Input Parameters:
*                *b_p   - pointer to the input bit stream
*                length - length of input bit stream (for one symbol)
*                       - 84 for header
*                       - 96*(mod+1) for data, where
*                         mod  - modulation (0-DBPSK; 1-DQPSK; 2-D8PSK)
*
* Output Parameters:
*                *x_p   - pointer to output bit stream
*
* Functions Called:
*
******************************************************************************/
void PHY_scm(Uint16 *b_p, Uint16 *x_p, Uint16 length)
{
	int i;

	Uint16 *pnLutEnd = (Uint16 *)PHY_pref + 126;
	Uint16 delta = pnLutEnd - PHY_scm_handle_s.pnScm_p + 1;

	if (length <= delta)
	{
		/* No wrap-round */
		for (i = 0; i < length; i++)
		{
			*x_p = (*b_p ^ *PHY_scm_handle_s.pnScm_p) & 1;

			x_p++;
			b_p++;
			PHY_scm_handle_s.pnScm_p++;
		}
	}
	else
	{
		/* There is wrap-round */
		/* First, do till the end of the table */
		for (i = 0; i < delta; i++)
		{
	        *x_p = (*b_p ^ *PHY_scm_handle_s.pnScm_p) & 1;

			x_p++;
			b_p++;
			PHY_scm_handle_s.pnScm_p++;
		}

		PHY_scm_handle_s.pnScm_p = (Uint16 *)PHY_pref;
		length -= delta;

		/* Next take care of the integer number of 127 bits */
		while (length >= 127)
		{
			for (i = 0; i < 127; i++)
			{
				*x_p = (*b_p ^ *PHY_scm_handle_s.pnScm_p) & 1;
				
				x_p++;
				b_p++;
				PHY_scm_handle_s.pnScm_p++;
			}
     
			PHY_scm_handle_s.pnScm_p = (Uint16 *)PHY_pref;
			length -= 127;
		}

		/* Last, the remain bits */
		for (i = 0; i < length; i++)
		{
	        *x_p = (*b_p ^ *PHY_scm_handle_s.pnScm_p) & 1;
			
			x_p++;
			b_p++;
			PHY_scm_handle_s.pnScm_p++;
		}
	}
}

/******************************************************************************
* FUNCTION NAME: PHY_mod_pnLoad
*
* DESCRIPTION:   Reload PN sequence for modulation pilots
*
* Return Value:  None
*
* Input Parameters:     
*
* Output Parameters:
* Functions Called:     
*
******************************************************************************/
void PHY_mod_pnLoad()
{
	PHY_mod_handle_s.pnMod_p = (Uint16 *)PHY_pref;
}

/******************************************************************************
* FUNCTION NAME: PHY_descm_pnLoad
*
* DESCRIPTION:   Reload PN sequence for scrambler
*
* Return Value:  None
*
* Input Parameters:
*
* Output Parameters:
* Functions Called:
*
******************************************************************************/
void PHY_descm_pnLoad()
{
	PHY_descm_handle_s.pnScm_p = (Uint16 *)PHY_pref;
}

/******************************************************************************
* FUNCTION NAME: PHY_descm
*
* DESCRIPTION:   Generate descrambled soft bits
*
* Return Value:  None
*
* Input Parameters:
*                *in_p - pointer to the input soft bits
*                numSoftBits  - number of soft bits
*                             - 84 for header
*                             - 96*(mod+1) for data; mod=0/1/2 for DBPSK/DQPSK/D8PSK
*
* Output Parameters:
*                *bd_p - pointer to output buffer struct for soft bits
*
* Functions Called:
*
******************************************************************************/
PRIME_status_t PHY_descm(int16 *in_p, Buffer_t *bd_p, Uint16 numSoftBits)
{
	int i;

	Uint16 *pnLutEnd = (Uint16 *)PHY_pref + 126;
	SINT16 *out_p, *outEnd_p;

	/* Check for space available */
	SINT16 space = bd_p->rdIdx - bd_p->wtIdx;
	if (space <= 0) space += bd_p->bufSize;
	if (space < numSoftBits) return PRIME_STAT_ERROR;

	out_p    = bd_p->buf_p + bd_p->wtIdx;
	outEnd_p = bd_p->buf_p + bd_p->bufSize - 1;

	for (i = 0; i < numSoftBits; i++)
	{
		if (*PHY_descm_handle_s.pnScm_p++ == 1)
		{
			*out_p++ = -(*in_p++);
		}
		else
		{
			*out_p++ = *in_p++;
		}

		/* checks for wrap around, might be more efficient if sep loops */
		if (PHY_descm_handle_s.pnScm_p > pnLutEnd)
		{
			PHY_descm_handle_s.pnScm_p = (Uint16 *)PHY_pref;
		}

		if (out_p > outEnd_p)
		{
			out_p = bd_p->buf_p;
		}
	}
  
	/* update write index */
	bd_p->wtIdx += numSoftBits;
  
	if (bd_p->wtIdx >= bd_p->bufSize)
	{
		bd_p->wtIdx -= bd_p->bufSize;
	}

	return PRIME_STAT_SUCCESS;
}

/******************************************************************************
* FUNCTION NAME: PHY_mod_genPld
*
* DESCRIPTION:   Generate modulated payload
*
* Return Value:  None
*
* Input Parameters: 
*                *b_p - pointer to the input bit stream
*                mod  - modulation (0-DBPSK; 1-DQPSK; 2-D8PSK)    
*
* Output Parameters:
*                *x_p - pointer to complex modulated data array
*
* Functions Called:     
*
******************************************************************************/
void PHY_mod_genPld(Uint16 *b_p, ComplexShort *x_p, Uint16 mod)
{
	int i;

	/* First tone is a pilot */
	x_p->real = 32767 * PHY_dbpsk[*PHY_mod_handle_s.pnMod_p];
	x_p->imag = 0;
	x_p++;

	PHY_mod_handle_s.pnMod_p++;

	/* PN wrap around */
	if ((PHY_mod_handle_s.pnMod_p - PHY_pref) >= 127)
	{
		PHY_mod_handle_s.pnMod_p = (Uint16 *)PHY_pref;
	}

	/* DBPSK */
	for (i = 1; i < 49; i++)
	{
		x_p->real = (x_p - 1)->real * PHY_dbpsk[*b_p];
		x_p->imag = 0;

		x_p++;
		b_p++;
	}
}

/******************************************************************************
* FUNCTION NAME: PHY_mod_genHdr
*
* DESCRIPTION:   Generate modulated header (always DBPSK)
*
* Return Value:  None
*
* Input Parameters:     
*                *b_p - pointer to the input bit stream
*
* Output Parameters:
*                *x_p - pointer to complex modulated data array
*
* Functions Called:     
*
******************************************************************************/
void PHY_mod_genHdr(Uint16 *b_p, ComplexShort *x_p)
{
	int i, k;

	/* PN wrap around */
	if ((PHY_mod_handle_s.pnMod_p - PHY_pref) >= 127)
	{
		PHY_mod_handle_s.pnMod_p = (Uint16 *)PHY_pref;
	}

	for (k = 0; k < 6; k++)
	{
		x_p->real = 32767 * PHY_dbpsk[*(PHY_mod_handle_s.pnMod_p++)];
		x_p->imag = 0;

		x_p++;

		if ((PHY_mod_handle_s.pnMod_p - PHY_pref) >= 127)
		{
			PHY_mod_handle_s.pnMod_p = (Uint16 *)PHY_pref;
		}

		for (i = 1; i < 8; i++)
		{
			/* BPSK for header: x[k]=x[k-1]*d
				b=0->theta=0(d=1); b=1->theta=180(d=-1) */
			x_p->real = (x_p - 1)->real * PHY_dbpsk[(*b_p++)];
			x_p->imag = 0;
  
			*x_p++;
		}
	}

	/* The last one is always a pilot */
	x_p->real = 32767 * PHY_dbpsk[*(PHY_mod_handle_s.pnMod_p++)]; 
	x_p->imag = 0;
}

/******************************************************************************
* FUNCTION NAME: PHY_eqz_genHdr
*
* DESCRIPTION:   Generate equalizer for header (always DBPSK)
*
* Return Value:  None
*
* Input Parameters:
*                *y_p - pointer to the received symbol array (FFT output). Q1.7 (8 bits)
*
* Output Parameters:
*                *llr_p - pointer to soft symbol array. Q1.4 (5 bits)
*
* Functions Called:
*
******************************************************************************/
void PHY_eqz_genHdr(ComplexShort *y_p, int16 *gama_p)
{
	int i;

	int16 *n2_p = (int16 *)y_p;
	ComplexShort *u_p = PHY_eqz_data_s.u;

	/* Input is cplx */
	/* First tone is a pilot */
	ComplexShort y0 = *y_p++;

	for (i = 1; i < 49; i++)
	{
		ComplexShort z, u, n;
		ComplexShort y1 = *y_p++;

		/* Every 8th tone is a pilot, no need to compute */
		if ((i & 7) != 0)
		{
   			int16 gama0;

			/* z=y_i*conj(y_(i-1)) */
			z.real = (SINT16)(((int32)y1.real * (int32)y0.real + (int32)y1.imag * (int32)y0.imag) >> 1); //Q2.13
			z.imag = (SINT16)(((int32)y1.imag * (int32)y0.real - (int32)y1.real * (int32)y0.imag) >> 1); //Q2.13

			/* Soft slicer */
			/* DBPSK */
			gama0 = 2 * z.real; //Q3.13

			/* Hard symbol */
			/* u=sign(z.real), in Q14 */
			u.real = (1 + ((z.real >> 15) << 1)) << 14;
			u.imag = 0;

			/* Find NVE */
			/* Noise: n(i)=y(i)-y(i-1)*u */
			n.real = (SINT16)(y1.real - (((int32)u.real * (int32)y0.real) >> 14)); //Q7
			n.imag = (SINT16)(y1.imag - (((int32)u.real * (int32)y0.imag) >> 14)); //Q7
			*n2_p++ =(SINT16)((int32)n.real * (int32)n.real + (int32)n.imag * (int32)n.imag); //Q14

			/* llr = gama0 */
			*gama_p++ = gama0;

			/* For debug */
			u_p->real = u.real;
			u_p->imag = u.imag;
		}
	
		/* */
		y0 = y1;
	}
}

/******************************************************************************
* FUNCTION NAME: PHY_eqz_genHdr_llr
*
* DESCRIPTION:   Generate equalizer for header (always DBPSK)
*
* Return Value:  None
*
* Input Parameters:
*                *sigma2_inv_p - pointer to 1/(sigma^2) (Q15)
*
* Output Parameters:
*                *llr_p - pointer to soft symbol array. Q1.4 (5 bits)
*
* Functions Called:
*
******************************************************************************/
void PHY_eqz_genHdr_llr(UINT16 *sigma2_inv_p, int16 *llr_p)
{
	int i;

	int16 *gama_p = llr_p;

	/* Compute LLR */
	for (i = 0; i < 42; i++)
	{
		/* gama is Q13, sigma2_inv is Q15, result in Q5 */
		SINT32 llrTmp = ((int32)(*gama_p++) * (int32)(*sigma2_inv_p++)) >> PHY_rxDbg_s.llrShift;

		*llr_p++ = (int16)__IQsat(llrTmp, 31, -32);
	}
}

/******************************************************************************
* FUNCTION NAME: PHY_eqz_genPld
*
* DESCRIPTION:   Generate equalizer for payload
*
* Return Value:  None
*
* Input Parameters:
*                *y_p - pointer to the received symbol array (FFT output)
*                mod  - modulation (0-DBPSK; 1-DQPSK; 2-D8PSK)
*
* Output Parameters:
*                *llr_p - pointer to soft symbol array
*
* Functions Called:
*
******************************************************************************/
void PHY_eqz_genPld(ComplexShort *y_p, int16 *gama_p, Uint16 mod)
{
	int i;

	int16 *n2_p = (int16 *)y_p;
	ComplexShort *u_p = PHY_eqz_data_s.u; 

	/* Input is cplx */
	/* First tone is a pilot */
	ComplexShort y0 = *y_p++;

	for (i = 1; i < 49; i++)
	{
		int16 gama0;
		ComplexShort z, u, n;
		ComplexShort y1 = *y_p++;

		/* z=y_i*conj(y_(i-1)) */
		/* y0, y1 in Q7 */
		z.real = (SINT16)(((int32)y1.real * (int32)y0.real + (int32)y1.imag * (int32)y0.imag) >> 1); //Q2.13
		z.imag = (SINT16)(((int32)y1.imag * (int32)y0.real - (int32)y1.real * (int32)y0.imag) >> 1); //Q2.13

		/* Soft slicer */

		/* DBPSK */
		/* z is Q13 */
		gama0 = 2 * z.real; //Q3.13

		/* Hard symbol */
		/* u=sign(z.real), in Q14 */
		u.real = (1 + ((z.real >> 15) << 1)) << 14;
		u.imag = 0;

		/* Find NVE */
		/* Noise: n(i)=y(i)-conj(y(i-1))*u */
		n.real = (SINT16)(y1.real - (((int32)u.real * (int32)y0.real) >> 14)); //Q7
		n.imag = (SINT16)(y1.imag - (((int32)u.real * (int32)y0.imag) >> 14)); //Q7
		*n2_p++ = (SINT16)((int32)n.real * (int32)n.real + (int32)n.imag * (int32)n.imag); //Q14

		/* llr = gama0 */
		*gama_p++ = gama0;

		/* For debug purpose */
		u_p->real = u.real;
		u_p->imag = u.imag;
		u_p++;

		/* */
		y0 = y1;
	}

  /* Call NVE */
}

/******************************************************************************
* FUNCTION NAME: PHY_eqz_genPld_llr
*
* DESCRIPTION:   Generate equalizer for payload (DBPSK/DQPSK/D8PSK)
*
* Return Value:  None
*
* Input Parameters:
*                *sigma2_inv_p - pointer to 2/(sigma^2) (Q15)
*                mod           - modulation (0/1/2 - DBPSK/DQPSK/D8PSK)
*
* Output Parameters:
*                *llr_p - pointer to soft symbol array. Q1.4 (5 bits)
*
* Functions Called:
*
******************************************************************************/
void PHY_eqz_genPld_llr(UINT16 *sigma2_inv_p, int16 *llr_p, Uint16 mod)
{
	int i;

  	int16 * gama_p = llr_p;

  	/* Compute LLR */

	/* DBPSK */
	for (i = 0; i < 96; i++)
	{
		/* gama is Q13, sigma2_inv is Q15, result in Q5 */
		SINT32 llrTmp = ((int32)(*gama_p++) * (int32)(*sigma2_inv_p++)) >> PHY_rxDbg_s.llrShift;

		*llr_p++ = (int16)__IQsat(llrTmp, 31, -32);
	}
}

/******************************************************************************
* FUNCTION NAME: PHY_intlv
*
* DESCRIPTION:   This function performs interleaving specified in prime standard.
*
*                Ncbpsc = 2 x Nbpsc, Nbpsc=0.5 (DBPSK), 1(DQPSK), 3(D8PSK)
*                s = 8x(1+floor(Ncbpsc/2))   for payload
*                s = 7                       for header
*                
*                w = interleaved vector, v = input vector
*                w((Ncbps/s)x(k mod s) + floor(k/s)) = v(k)
*                                                                      
*                It fills in column and reads out row.
*
* Input Parameters:    nBits - number of input bits for interleaving                         
*                      in_p  - pointer to input bits                                    
* Output Parameters:   out_p - pointer to output interleaved bits
*
******************************************************************************/
void PHY_intlv(UINT16 nBits, UINT16 *in_p, UINT16 *out_p, UINT16 idx)
{
	int i, j;

	SINT16 nRows = PHY_intlvCfg_s.nRows[idx];
	SINT16 nCols = PHY_intlvCfg_s.nCols[idx];

	for (i = 0; i < nRows; i++)
	{
		UINT16 *in2_p = in_p;
		
		for (j = 0; j < nCols; j++)
		{
			*out_p = *in2_p;

			out_p++;
			in2_p += nRows;
		}

		in_p++;
    }
}

/******************************************************************************
* FUNCTION NAME: PHY_intlvBlk
*
* DESCRIPTION:   This function performs interleaving specified in prime standard.
*
*                Ncbpsc = 2 x Nbpsc, Nbpsc=0.5 (DBPSK), 1(DQPSK), 3(D8PSK)
*                s = 8x(1+floor(Ncbpsc/2))   for payload
*                s = 7                       for header
*                
*                w' = de-interleaved i/p vector, v' = output
*                v'(sxi-(Ncbpsc-1)xfloor(sxi/Ncbpsc)) = w'(i)
*                                                                      
*                It fills in row and reads out column.
*
* Input Parameters:    nBits - number of input bits for de-interleaving                         
*                      in_p  - pointer to input bits                                    
* Output Parameters:   out_p - pointer to output de-interleaved bits
*
******************************************************************************/
void PHY_intlvBlk(UINT16 nBits, UINT16 *in_p, UINT16 *out_p, UINT16 idx)
{
	int i, j;

	int nRows = PHY_intlvBlkCfg_s.nRows[idx];
	int nCols = PHY_intlvBlkCfg_s.nCols[idx];

	for (i = 0; i < nRows; i++)
    {
		UINT16 *in2_p = in_p;
      
		for (j = 0; j < nCols; j++)
		{
			*out_p++ = *in2_p;
			in2_p += nRows;
		}
      
		in_p++;
    }
}

/******************************************************************************
* FUNCTION NAME: PHY_dtlvBlk
*
* DESCRIPTION:   This function performs interleaving specified in prime standard.
*
*                Ncbpsc = 2 x Nbpsc, Nbpsc=0.5 (DBPSK), 1(DQPSK), 3(D8PSK)
*                s = 8x(1+floor(Ncbpsc/2))   for payload
*                s = 7                       for header
*                
*                w' = de-interleaved i/p vector, v' = output
*                v'(sxi-(Ncbpsc-1)xfloor(sxi/Ncbpsc)) = w'(i)
*                                                                      
*                It fills in row and reads out column.
*
* Input Parameters:    nBits - number of input bits for de-interleaving                         
*                      in_p  - pointer to input bits                                    
* Output Parameters:   out_p - pointer to output de-interleaved bits
*
******************************************************************************/
void PHY_dtlvBlk(UINT16 nBits, SINT16 *in_p, SINT16 *out_p, UINT16 idx)
{
	int i, j;

	int nRows = PHY_intlvBlkCfg_s.nRows[idx];
	int nCols = PHY_intlvBlkCfg_s.nCols[idx];

	for (i = 0; i < nRows; i++)
    {
		SINT16 *out2_p = out_p;
		
		for (j = 0; j < nCols; j++)
        {
			*out2_p = *in_p++;
			out2_p += nRows;
        }
		out_p++;
    }
}

/******************************************************************************
* FUNCTION NAME: PHY_dtlv
*
* DESCRIPTION:   This function performs interleaving specified in prime standard.
*
*                Ncbpsc = 2 x Nbpsc, Nbpsc=0.5 (DBPSK), 1(DQPSK), 3(D8PSK)
*                s = 8x(1+floor(Ncbpsc/2))   for payload
*                s = 7                       for header
*                
*                w' = de-interleaved i/p vector, v' = output
*                v'(sxi-(Ncbpsc-1)xfloor(sxi/Ncbpsc)) = w'(i)
*                                                                      
*                It fills in row and reads out column.
*
* Input Parameters:    nBits - number of input bits for de-interleaving                         
*                      in_p  - pointer to input bits                                    
* Output Parameters:   out_p - pointer to output de-interleaved bits
*
******************************************************************************/
void PHY_dtlv(UINT16 nBits, SINT16 *in_p, SINT16 *out_p, UINT16 idx)
{
	int i, j;

	SINT16 nRows = PHY_intlvCfg_s.nRows[idx];
	SINT16 nCols = PHY_intlvCfg_s.nCols[idx];

	for (i = 0; i < nRows; i++)
	{
		SINT16 *out2_p = out_p;
	  
		for (j = 0; j < nCols; j++)
		{
			*out2_p = *in_p++;

	  		out2_p += nRows;
		}
	  
		out_p++;
	}
}

void PHY_firCplxInUp8Cvt(UINT16 nSamps, PHY_fir_t *fir_p, SINT16 *in_p, SINT16 *out_p) 
{
	int i, j, k, off = 0;

	for(i = 0; i < nSamps; i++)
	{
		for(j = 0; j < 8; j++)
		{
			SINT32 sum_real = 0;
			SINT32 sum_imag = 0;
			SINT32 temp, temp_real, temp_imag;

			for(k = 0; k < 4; k++)
			{
				sum_real += fir_p->coeff_p[j*8+2*k] * in_p[2*i+2*k]; 
				sum_imag += fir_p->coeff_p[j*8+2*k+1] * in_p[2*i+2*k+1]; 
			}
			
			temp_real = (sum_real >> 12) * PHY_rxDnCvtLut[off].real;
			temp_imag = (sum_imag >> 12) * PHY_rxDnCvtLut[off].imag;
			temp = (temp_real + temp_imag + 2048) >> 12;
		
			out_p[i*8+j] = (SINT16)temp;

			off = (off + 1) % 128;
		}
	}
}

/******************************************************************************
* FUNCTION NAME: PHY_firPReImInDn4
*
* DESCRIPTION:   This function performs front end processing during preamble
*                detection in hunt state
*
*
* Input Parameters:    
* Output Parameters:   
*
******************************************************************************/
void PHY_firPReImInDn4(UINT16 nSamps, PHY_fir_t *fir_p, SINT16 *in_p, ComplexShort *out_p)
{
	SINT16 i, j;

	SINT16 *coeff_p = (SINT16 *)fir_p->coeff_p;

	in_p++;

	for(i = 0; i < nSamps/4; i++)
	{
		long sum1 = 0;
		long sum2 = 0;

		for (j = 0; j < fir_p->nTaps/2; j++)
		{
			sum1 += *coeff_p * (*in_p);
			in_p++;
			coeff_p++;

			sum2 += *coeff_p * (*in_p);
			in_p++;
			coeff_p++;

			if (coeff_p > (fir_p->coeff_p+64))
			{
				coeff_p = fir_p->coeff_p;
			}
		}

		in_p -= 12;

		out_p[i].imag = (SINT16)(sum1 >> 8);
		out_p[i].real = (SINT16)(sum2 >> 8);
	}
}

/******************************************************************************
* FUNCTION NAME: PHY_flexCfg
*
* DESCRIPTION:   This function configures for FLEX PRIME
*                 
*
* Return Value:
*
* Input Parameters:    startIdx - start tone index
*                      endIdx   - end tone index (now only support 25 tones tot)
* Output Parameters:
*
******************************************************************************/
void PHY_flexCfg(UINT16 startIdx)
{
	HAL_afe_prfParms_t afePrfParms;

	UINT16 endIdx = startIdx + 48;

	PHY_bandCfg_s.startIdx = startIdx;
	PHY_bandCfg_s.endIdx   = endIdx;

	/* B-band starts from 98.4kHz, or 201th tone */
	if (endIdx >= 201)
	{
		afePrfParms.band = 1;
	}
	else
	{
		afePrfParms.band = 0;
	}

	/* center tone = (startIdx + endIdx)/2, but need to be 8x */
	/* then LUT size is 1024/8 = 128                          */
	PHY_bandCfg_s.centerIdx = ((startIdx + endIdx + 8) / 16) * 8;
	PHY_bandCfg_s.delta = -(SINT32)PHY_bandCfg_s.centerIdx << 18;

	/* Initialize LUT for downconversion - based on band cfg later */
	MATH_blockCosSin(128, 0, PHY_bandCfg_s.delta, PHY_rxDnCvtLut);

  // Need to generate preambles, for dnd (rx in freq.) and raw 128-p cplx (tx, need to ups->upCvt)

	/* Compute fft rotate tones */
	PHY_bandCfg_s.numLoTones = PHY_bandCfg_s.centerIdx - startIdx;
	PHY_bandCfg_s.numHiTones = endIdx - PHY_bandCfg_s.centerIdx + 1; 
      
	/* lo start = fft - numLoTones, lo end = fft */
	/* hi start = 0, hi end = numHiTones - 1*/
	PHY_bandCfg_s.loStartIdx = 128 - PHY_bandCfg_s.numLoTones;
}

/******************************************************************************
* FUNCTION NAME: UTIL_blockPack2Word
*
* DESCRIPTION:   This function pack bits to 16-bits word
*
* Input Parameters:    nBits   - number of bits (multiple of 16-bits)
*                      offset  - byte offset for output
*                      in_p    - pointer to input data in bits
*                      
*
* Output Parameters:   out_p   - pointer to output data in 16-bits word           
*
******************************************************************************/
void UTIL_blockPack2Word(UINT16 nBits, UINT16 offset, UINT16 *in_p, UINT16 *out_p)
{
	int i, k;
	SINT16 nBytes = nBits >> 3;

	if (offset)  nBytes++;

	for (i = offset; i < nBytes; i++)
	{
		UINT16 tmp = *in_p++;
  
		for (k = 0; k < 7; k++)
		{
			tmp <<= 0x1;
			tmp |= *in_p++;
		}
  
		out_p[i] = tmp;
	}
}

/******************************************************************************
* FUNCTION NAME: UTIL_ShortComplexDotProduct
*
* DESCRIPTION:   This function computes the dot product of two complex vectors.
*                The product is right shifted by "shift" bits,
*
* Input Parameters:    nSize    - number of data to multiply
*                      src_p    - pointer to src data                
*                      m_p      - pointer to multiplicand vector 
*                      result_p - pointer to the result
*                      shift    - product left shift
* Output Parameters:   
*
******************************************************************************/
void UTIL_ShortComplexDotProduct(UINT16 nSize, ComplexShort *src_p, ComplexShort *m_p, ComplexShort *result_p, UINT16 shift)
{
	int i;
	long x = 0, y = 0;

	for (i = 0; i < nSize; i++)
	{
		/* conjugate complex multiplication and accumulation */
		x += (long)src_p->real * (long)m_p->real + (long)src_p->imag * (long)m_p->imag;
		y += (long)src_p->real * (long)m_p->imag - (long)src_p->imag * (long)m_p->real;

		src_p++;
		m_p++;
	}
  
	result_p->real = (SINT16)(x >> shift);
	result_p->imag = (SINT16)(y >> shift);
}

/******************************************************************************
* FUNCTION NAME: UTIL_ShortComplexConjMult
*
* DESCRIPTION:   This function multiplies a real and a complex short block.
*                The product is right shifted by "shift" bits,
*
* Input Parameters:    nSize    - number of data to multiply
*                      src_p    - pointer to src data
*                      m_p      - pointer to multiplicand vector
*                      dst_p    - pointer to dst data
*                      shift    - product left shift
* Output Parameters:
*
******************************************************************************/
void UTIL_ShortComplexConjMult(UINT16 nSize, ComplexShort *src_p, const ComplexShort *m_p, ComplexShort *dst_p, UINT16 shift)
{
	int i;

	for (i=0; i<nSize; i++)
	{
		  long x = (((long)src_p->real * (long)m_p->real) + ((long)src_p->imag * (long)m_p->imag)) >> shift;
		  long y = (((long)src_p->real * (long)m_p->imag) - ((long)src_p->imag * (long)m_p->real)) >> shift;

		  dst_p[i].real = (SINT16)x;
		  dst_p[i].imag = (SINT16)y;
  
		  src_p++;
		  m_p++;
	}
}

/******************************************************************************
* FUNCTION NAME: UTIL_blockShortFill
*
* DESCRIPTION:   This function performs block copy of short type data (16-bits)
*
* Input Parameters:    nSize   - number of short to copy
*                      src_p   - pointer to src buffer
* Output Parameters:   dst_p   - pointer to dst buffer
*
******************************************************************************/
void UTIL_blockShortFill(UINT16 nSize, SINT16 *dst_p, SINT16 val)
{
	int i;

	for (i=0; i<nSize; i++)
	{
		*dst_p++ = val;
	}
}

/******************************************************************************
* FUNCTION NAME: UTIL_blockShortCopy
*
* DESCRIPTION:   This function performs block copy of short type data (16-bits)
*
* Input Parameters:    nSize   - number of short to copy
*                      src_p   - pointer to src buffer
* Output Parameters:   dst_p   - pointer to dst buffer
*
******************************************************************************/
void UTIL_blockShortCopy(UINT16 nSize, SINT16 *src_p, SINT16 *dst_p)
{
	int i;

	for (i = 0; i < nSize; i++)
	{
		*dst_p++ = *src_p++;
	}
}

/******************************************************************************
* FUNCTION NAME: UTIL_blockShortPower
*
* DESCRIPTION:   This function performs front end processing during preamble
*                detection in hunt state
*
*
* Input Parameters:    
* Output Parameters:   
*
******************************************************************************/
long UTIL_blockShortPower(UINT16 nSamps, SINT16 *in_p)
{
	int i;
	long power = 0;

	for(i = 0; i < nSamps; i++)
	{
      power += (*in_p) * (*in_p);

      in_p++;
	}
	
	return power;
}

/******************************************************************************
* FUNCTION NAME: UTIL_blockLongCircularCopy
*
* DESCRIPTION:   This function performs front end processing during preamble
*                detection in hunt state
*
*
* Input Parameters:    
* Output Parameters:   
*
******************************************************************************/
void UTIL_blockLongCircularCopy(UINT16 nSize, SINT32 *src_p, SINT32 *dst_p, UINT16 bufSize)
{
	int i;

	for (i = 0; i < nSize; i++)
	{
		*dst_p = *src_p;
		src_p++;
		dst_p++;
	}
}

/******************************************************************************
* FUNCTION NAME: UTIL_blockShortComplexPower
*
* DESCRIPTION:   This function performs front end processing during preamble
*                detection in hunt state
*
*
* Input Parameters:    
* Output Parameters:   
*
******************************************************************************/
long UTIL_blockShortComplexPower(UINT16 nSamps, SINT16 *in_p)
{
	int i;
	long power = 0;

	for (i = 0; i < nSamps; i++)
	{
		power += (*in_p) * (*in_p);

		in_p++;
	}
	
	return power;
}

/******************************************************************************
* FUNCTION NAME: UTIL_blockShortMean
*
* DESCRIPTION:   This function performs front end processing during preamble
*                detection in hunt state
*
*
* Input Parameters:    
* Output Parameters:   
*
******************************************************************************/
SINT16 UTIL_blockShortMean(UINT16 nSampsInLog2, SINT16 *in_p)
{
	SINT16  i, mean;
	SINT32 sum = 0;

	for(i = 0; i < (1 << nSampsInLog2); i++)
	{
		sum += in_p[i];
	}

	mean = (SINT16) (sum >> nSampsInLog2);

	return mean;
}

/******************************************************************************
* FUNCTION NAME: UTIL_blockShortOffset
*
* DESCRIPTION:   This function performs front end processing during preamble
*                detection in hunt state
*
*
* Input Parameters:    
* Output Parameters:   
*
******************************************************************************/
void UTIL_blockShortOffset(UINT16 nSize, SINT16 *data_p, SINT16 val)
{
	SINT16 i;

	for (i = 0; i < nSize; i++)
	{
		data_p[i] += val;
	}
}


/******************************************************************************
* FUNCTION NAME: UTIL_blockLongFill
*
* DESCRIPTION:   This function performs block copy of long type data (32-bits)
*
* Input Parameters:    nSize   - number of short to copy
*                      src_p   - pointer to src buffer
* Output Parameters:   dst_p   - pointer to dst buffer
*
******************************************************************************/
void UTIL_blockLongFill(UINT16 nSize, SINT32 *dst_p, SINT32 val)
{
	SINT16 i;

	for (i=0; i<nSize; i++)
	{
		*dst_p++ = val;
	}
}

/******************************************************************************
* FUNCTION NAME: UTIL_blockShortComplexMult
*
* DESCRIPTION:   This function multiplies a real and a complex short block.
*                The product is right shifted by "shift" bits,
*
* Input Parameters:    nSize    - number of data to multiply
*                      src_p    - pointer to src data                
*                      m_p      - pointer to multiplicand vector 
*                      dst_p    - pointer to dst data                
*                      shift    - product left shift
* Output Parameters:   
*
******************************************************************************/
void UTIL_blockShortComplexMult(UINT16 nSize, ComplexShort *src_p, const ComplexShort *m_p, ComplexShort *dst_p, UINT16 shift)
{
	SINT16 i;

	for (i = 0; i < nSize; i++)
	{
		 long x = (((long)src_p->real * (long)m_p->real) - ((long)src_p->imag * (long)m_p->imag)) >> shift;
		 long y = (((long)src_p->real * (long)m_p->imag) + ((long)src_p->imag * (long)m_p->real)) >> shift;

		 dst_p[i].real = (SINT16)x;
		 dst_p[i].imag = (SINT16)y;
  
		 src_p++;
		 m_p++;
	}
}

/******************************************************************************
* FUNCTION NAME: UTIL_blockLongCopy
*
* DESCRIPTION:   This function performs block copy of long type data (32-bits)
*
* Input Parameters:    nLongs - number of long to copy
*                      src_p   - pointer to src buffer
* Output Parameters:   dst_p   - pointer to dst buffer
*
******************************************************************************/
void UTIL_blockLongCopy(UINT16 nSize, SINT32 *src_p, SINT32 *dst_p)
{
	SINT16 i;

	for (i = 0; i < nSize; i++)
	{
		*dst_p++ = *src_p++;
	}
}

/******************************************************************************
* FUNCTION NAME: UTIL_blockUnpack2Bits
*
* DESCRIPTION:   This function unpack to bits
*
* Input Parameters:    nSize   - number of 16-bits words 
*                      in_p    - pointer to input data in 16-bit words
*                      out_p   - pointer to output data in bits           
*
* Output Parameters:   
*
******************************************************************************/
void UTIL_blockUnpack2Bits(UINT16 nSize, UINT16 *in_p, UINT16 *out_p)
{
	SINT16 i;

	for (i = 0; i < nSize; i++)
	{
		UINT16 data = in_p[i];
		UINT16 data_byte = data & 0xFF;
	  
		*out_p++ = (data_byte >>7) & 0x1;
		*out_p++ = (data_byte >>6) & 0x1;
		*out_p++ = (data_byte >>5) & 0x1;
		*out_p++ = (data_byte >>4) & 0x1;
		*out_p++ = (data_byte >>3) & 0x1;
		*out_p++ = (data_byte >>2) & 0x1;
		*out_p++ = (data_byte >>1) & 0x1;
		*out_p++ = (data_byte >>0) & 0x1;

		data_byte = (data >> 8) & 0xFF;
	  
		*out_p++ = (data_byte >>7) & 0x1;
		*out_p++ = (data_byte >>6) & 0x1;
		*out_p++ = (data_byte >>5) & 0x1;
		*out_p++ = (data_byte >>4) & 0x1;
		*out_p++ = (data_byte >>3) & 0x1;
		*out_p++ = (data_byte >>2) & 0x1;
		*out_p++ = (data_byte >>1) & 0x1;
		*out_p++ = (data_byte >>0) & 0x1;
	}
}

/******************************************************************************
* FUNCTION NAME: UTIL_blockLongCopy
*
* DESCRIPTION:   This function performs block copy of long type data (32-bits)
*
* Input Parameters:    nLongs - number of long to copy
*                      src_p   - pointer to src buffer
* Output Parameters:   dst_p   - pointer to dst buffer
*
******************************************************************************/
void UTIL_blockCmplxShortCopy(UINT16 nSize, SINT32 *src_p, SINT32 *dst_p)
{
	SINT16 i;

	for (i=0; i<nSize; i++)
	{
		*dst_p++ = *src_p++;
	}
}

/******************************************************************************
* FUNCTION NAME: UTIL_blockShortSat
*
* DESCRIPTION:   This function saturates the short block data to maxVal/minVal.
*
* Input Parameters:    nSize    - number of data to multiply
*                      data_p   - pointer to data                
*                      maxVal   - limit to max
*                      minVal   - limit to min
* Output Parameters:   
*
******************************************************************************/
void UTIL_blockShortComplexSat(UINT16 nSize, SINT16 *data_p, SINT16 maxVal, SINT16 minVal)
{
	SINT16 i;

	for (i = 0; i < (nSize<<1); i++)
	{
		long x = __IQsat((long)(*data_p), maxVal, minVal);

		*data_p++ = (SINT16)x;
	}
}

void __norm32(SINT32 value, SINT16 *out_k)
{
	int i;

	for (i = 30; i > 0; i--)
	{
		SINT32 mask = (1 << i);

		if (value & mask)
		{
			break;
		}
	}

	*out_k = 30-i;
}

/******************************************************************************
* FUNCTION NAME: MATH_Q0linearToQ4dB
*
* DESCRIPTION: This function converts +ve value in linear (Q0) to dB (Q4). 
*              Its accurate to <0.25 dB. 
*              If input <= 0, returns 0x80000000
*
* Return Value:     dB = 10*log10(value)
*
* Input Parameters:  

*
* Output Parameters:
* Functions Called:
*
******************************************************************************/
SINT32 MATH_Q0linearToQ4dB(SINT32 value)
{
	SINT16 dB, idx, k;

	if (value <= 0)
	{
		return 0x80000000;
	}

	__norm32(value, &k);
	k = 30 - k;

	/* Convert to dB in Q4 */
	dB = 3 * k;
	idx = (SINT16)(value >> (k - 4));
	dB = (dB << 4) + MATH_dBLut[idx & 0xF];

	return dB;
}

/******************************************************************************
* FUNCTION NAME: MATH_Q4dBToQ12linear
*
* DESCRIPTION: This function computes the Q12 linear value for the given Q4 dB 
*              input. The steps are:
*
*               (1) special cases: if (x > kMaxQ4dBInput) return 0x7FFFFFFF. 
*               (2) normalize x:
*                       10^(x/10) = 2^[log2(10)*x/10] = 2^[k+z] = (2^z) * (2^k)
*                       find maximal integer k so that -0.5 <= z < 0.5
*               (3) approximate 2^z = e^[ln(2)*z] = e^v, abs(v) < 0.34657359
*                       e^v = 1 + v + v^2/2 + v^3/3 + v^4/4 + ...
*                       For abs(v) <= 0.34657359, first 4 terms give accuracy 
*                       better than 0.0001
*               (4) output = (e^v) << k
*
*
*       Note:
*               (1) Output is saturated to 0x7FFFFFFF when 
*                        input x > kMaxQ4dBInput = 915.
*               (2) The output's accuracy is limited to 0.06% with respect to the 
*                   true value when input is big, for negative input, the accuracy 
*                   is also limited by the Q12 output representation.
*
*    Note: Need to check of negative ????
*
* Return Value:     y = 10^(x_Q4dB/10) in Q12
*
* Input Parameters:   x_dB in Q4
* Output Parameters:  
* Functions Called:
*
******************************************************************************/
SINT32 MATH_Q4dBToQ12linear(UINT32 x)
{
	SINT32 k;
	SINT32 z, v, v1, v2, v3, v4, u;
    
	/* (1) special cases */
	if (x > MATH_MAX_Q4dB_INPUT) 
	{
		return 0x7FFFFFFF;
	}
    
	if (x == 0)
	{
		return 0xFFFFFFFF;
	}

	/* (2) normalize x */
	z = (MATH_LOG2_10_DIV10_Q16 * x) >> 4;          /* log2(10)*x/10 in Q16 */
	k = (z + 0x8000) >> 16;                         /* k is Q0 */
	z -= (k << 16);                                 /* z is Q16, and abs(z) <= 0.5 */

	/* (3) approximate 2^z */
	v = MATH_LN_2_Q14 * z;                          /* v in Q30 */

	v1 = v >> 15;                                   /* v in Q15 */
	v2 = (v1 * v1) >> 1;                            /* v^2/2 in Q30 */
	v3 = (v2 >> 15) * ((v1 * MATH_1_3_Q15) >> 15);  /* v^3/3 in Q30 */
	v4 = (v3 >> 15) * (v1 >> 2);                    /* v^4/4 in Q30 */

	u = MATH_1_Q30 + v + v2 + v3 + v4;              /* u = 2^z in Q30 */

	/* (4) output */
	return (u >> (18 - k));
}

/**************************************************************************************
* FUNCTION: MATH_blockCosSin
*
* DESCRIPTION:  This function calculates an array of  complex values 
*               cos(angle + i*delta) + jsin(angle + i*delta). The
*               formulas used for fine tuning are the same as for UtilCosSin(),
*               however the angle representation is different to avoid rounding
*               errors.
*
*               The angles representation is assumed to have 0x10000000 correspond
*               to 360 degrees or 2PI. With this representation, angles automatically wrap
*               around 0x10000000, and no distinction needs to be made about positive
*               or negative angles.
*
*
*               For returned cosine and sine values, 0x1000 represent floating point 1.0.
*
* Return Value:       output.real = cos(angle)
*                     output.imag = sin(angle)
*
* Input Parameters:   N     - number of input data
*                     angle - angle in radian (Q12, 4096=2pi)
*                     delta - angle increments
*                     dst_p - pointer to output array
*
* Output Parameters:  
* Functions Called:
*
*
*************************************************************************************/
SINT32 MATH_blockCosSin (SINT16 N, SINT32 angle, SINT32 delta, ComplexShort *dst_p)
{
	SINT16 i;
  
	for(i = 0; i < N; i++)
    {
		SINT16 x, y;
		UINT16 tempAngle = (angle + 0x8000) >> 16;       
		UINT16 tabIndex  = (tempAngle >> 4) & 0x3F;
		UINT16 residue   = (SINT16)(((tempAngle & 0x0F) * (SINT32)MATH_PI) >> 4);
      
		SINT16 cosA = MATH_cosLut[tabIndex];
		SINT16 sinA = MATH_cosLut[64-tabIndex];
      
		x = (SINT16)(cosA - ((residue * (SINT32)sinA) >> 19));
		y = (SINT16)(sinA + ((residue * (SINT32)cosA) >> 19));
      
		if ((tempAngle & 0x0800) != 0)
        {
			x = -x;
			y = -y;
        }

		if ((tempAngle & 0x0400) != 0)
        {
			sinA = y;
			y = x;
			x = -sinA;
        }

		angle += delta;
		angle &= 0x1FFFFFFF;

		dst_p[i].real = x;
		dst_p[i].imag = y;
    } 

	return angle;
}

/******************************************************************************
* FUNCTION NAME: MATH_divide32_16_q15
*
* DESCRIPTION:   This function performs interleaving specified in prime standard.
*
*                Ncbpsc = 2 x Nbpsc, Nbpsc=0.5 (DBPSK), 1(DQPSK), 3(D8PSK)
*                s = 8x(1+floor(Ncbpsc/2))   for payload
*                s = 7                       for header
*                
*                w = interleaved vector, v = input vector
*                w((Ncbps/s)x(k mod s) + floor(k/s)) = v(k)
*                                                                      
*                It fills in column and reads out row.
*
* Input Parameters:    nBits - number of input bits for interleaving                         
*                      in_p  - pointer to input bits                                    
* Output Parameters:   out_p - pointer to output interleaved bits
*
******************************************************************************/
UINT32 MATH_divide32_16_q15(UINT32 num, UINT16 den)
{
	UINT32 high, low;
	
	high = num % den;
	low = num / den;

	return ((high << 16) | low);
}
